﻿#pragma once

#include <xy/geom/vec.h>
#include <xy/solver/linear.h>

namespace xy
{

namespace alg
{

template <std::size_t N, typename T> struct bounding_box
{
    using vec_t = geom::vec<N, T>;
    using vecp_t = geom::vec<N - 1, T>;

    vec_t min, max, axis[N];
    std::vector<vecp_t> params;

    /**
     * @brief 轴对齐包围盒（Axis-Aligned Bounding Box, AABB）
     *
     * @param[in] points
     * @return bounding_box
     */
    static bounding_box AABB(const std::vector<vec_t> &points)
    {
        bounding_box bbox;
        bbox.min = bbox.max = points[0];
        for (auto &p : points)
        {
            for (std::size_t i = 0; i < N; ++i)
            {
                bbox.min[i] = std::min(bbox.min[i], p[i]);
                bbox.max[i] = std::max(bbox.max[i], p[i]);
            }
        }
        return bbox;
    }

    /**
     * @brief 定向包围盒（Oriented Bounding Box, OBB）
     *
     * @param[in] points
     * @return bounding_box
     */
    static bounding_box OBB(const std::vector<vec_t> &points)
    {
        // 坐标分离为 3 个随机变量
        std::size_t n = points.size();
        std::vector<T> X[N];
        for (std::size_t i = 0; i < N; i++)
        {
            X[i].resize(n);
            for (std::size_t j = 0; j < n; j++)
                X[i][j] = points[j][i];
        }

        // 计算期望点
        T E[N];
        for (std::size_t i = 0; i < N; i++)
            E[i] = std::accumulate(X[i].begin(), X[i].end(), T{}) / n;

        // 计算协方差矩阵
        math::matxd B(N, N);
        B.set_zero();

        for (std::size_t i = 0; i < n; i++)
            for (std::size_t j = 0; j < N; j++)
                for (std::size_t k = 0; k < N; k++)
                    B(j, k) += (X[j][i] - E[j]) * (X[k][i] - E[k]);
        B /= n;

        // 特征分解
        solver::linear_sym_eigen solver(std::move(B));
        solver.perform();
        auto V = solver.solve(n * 10);
        B = solver.move_matrix();

        // 比较对角元，进行排序
        std::size_t order[N];
        std::iota(order, order + N, 0);
        for (std::size_t i = 0; i < N; i++)
        {
            for (std::size_t j = 1; j < N; j++)
            {
                if (B(j, j) > B(j - 1, j - 1))
                {
                    std::swap(order[j], order[j - 1]);
                    std::swap(B(j, j), B(j - 1, j - 1));
                }
            }
        }

        // 三个主方向，按照从大到小的顺序
        bounding_box bbox;
        for (std::size_t i = 0; i < N; i++)
        {
            for (std::size_t j = 0; j < N; j++)
                bbox.axis[i][j] = V(j, order[i]);
            bbox.axis[i].normalize();
        }

        // 计算在主方向上的投影长度
        for (std::size_t i = 0; i < N; i++)
        {
            bbox.min[i] = std::numeric_limits<T>::max();
            bbox.max[i] = -std::numeric_limits<T>::max();
        }

        for (std::size_t i = 0; i < n; i++)
        {
            vec_t p;
            for (std::size_t j = 0; j < N; j++)
                p[j] = dot(points[i], bbox.axis[j]);

            // 在最大的两个主方向上投影
            vecp_t pp;
            for (std::size_t j = 0; j < N - 1; j++)
                pp[j] = p[j];
            bbox.params.emplace_back(pp);

            for (std::size_t j = 0; j < N; j++)
            {
                bbox.min[j] = std::min(bbox.min[j], p[j]);
                bbox.max[j] = std::max(bbox.max[j], p[j]);
            }
        }

        return bbox;
    }

    /**
     * @brief OBB 包围盒投影参数化
     *
     * @param[in] points
     * @return std::vector<vecp_t>
     */
    static std::vector<vecp_t> projection(const std::vector<vec_t> &points)
    {
        auto bbox = OBB(points);
        const auto &min = bbox.min;
        const auto &max = bbox.max;
        auto &params = bbox.params;
        for (std::size_t i = 0; i < points.size(); i++)
        {
            auto &p = params[i];
            for (std::size_t j = 0; j < N - 1; j++)
                p[j] = (p[j] - min[j]) / (max[j] - min[j]);
        }
        return bbox.params;
    }
};

} // namespace alg

} // namespace xy
